#include "RPathPlan_astar.h"
#include <vector>
#include <algorithm>
#include <random>
#include <cmath>

namespace rtk {

// 假设RMAP_OBSTACLE在RMap.h中有定义，这里先简单定义一个值用于示例
#ifndef RMAP_OBSTACLE
#define RMAP_OBSTACLE 1 
#endif

// GeneticData类函数实现
GeneticData::GeneticData(RMap* map) : m_map(map) {
    if (m_map != nullptr) {
        m_startIdx = map->getStartX() + map->getStartY() * map->getSizeX();
        m_endIdx = map->getEndX() + map->getEndY() * map->getSizeX();
        m_width = map->getSizeX();
        m_height = map->getSizeY();
        m_rng = std::mt19937(std::random_device{}());
    }
}

// 生成有效节点（非障碍物）
uint32_t GeneticData::generateValidNode() {
    if (m_map != nullptr) {
        int x, y;
        do {
            x = std::uniform_int_distribution<>(0, m_width - 1)(m_rng);
            y = std::uniform_int_distribution<>(0, m_height - 1)(m_rng);
        } while (m_map->getMapCell(x, y) == RMAP_OBSTACLE);
        return x + y * m_width;
    }
    return 0; // 如果m_map为空，返回一个默认值，可根据实际情况调整
}

// 初始化种群
Population GeneticData::initPopulation(int size) {
    Population pop;
    if (m_map != nullptr) {
        for (int i = 0; i < size; i++) {
            Chromosome chrom = {m_startIdx};
            for (int j = 0; j < 50; j++) chrom.push_back(generateValidNode());
            chrom.push_back(m_endIdx);
            validPath(chrom);
            pop.push_back(chrom);
        }
    }
    return pop;
}

// 适应度计算（路径长度+障碍物惩罚）
double GeneticData::calculateFitness(const Chromosome& chrom) {
    if (m_map != nullptr) {
        double cost = 0;
        for (size_t i = 1; i < chrom.size(); i++) {
            int x1 = chrom[i - 1] % m_width, y1 = chrom[i - 1] / m_width;
            int x2 = chrom[i] % m_width, y2 = chrom[i] / m_width;
            cost += std::hypot(x2 - x1, y2 - y1);
            if (m_map->getMapCell(chrom[i]) == RMAP_OBSTACLE) cost += 100;
        }
        return 1.0 / (cost + 1e-6);
    }
    return 0.0; // 如果m_map为空，返回一个默认值，可根据实际情况调整
}

// 锦标赛选择
Chromosome GeneticData::selection(const Population& pop, const std::vector<double>& fit) {
    std::vector<size_t> idx(pop.size());
    std::iota(idx.begin(), idx.end(), 0);
    std::shuffle(idx.begin(), idx.end(), m_rng);

    // 这里假设锦标赛规模为3，可根据需要调整
    size_t bestIdx = idx[0];
    for (size_t i = 1; i < 3; i++) {
        if (fit[idx[i]] > fit[bestIdx]) {
            bestIdx = idx[i];
        }
    }
    return pop[bestIdx];
}

// 验证路径有效性（这里增加一些简单的连通性检查逻辑）
void GeneticData::validPath(Chromosome& chrom) {
    if (chrom.size() < 2) return;
    for (size_t i = 1; i < chrom.size(); i++) {
        int x1 = chrom[i - 1] % m_width;
        int y1 = chrom[i - 1] / m_width;
        int x2 = chrom[i] % m_width;
        int y2 = chrom[i] / m_width;
        // 简单检查相邻节点是否连通（这里只考虑曼哈顿距离为1的情况）
        if (std::abs(x2 - x1) + std::abs(y2 - y1) > 1) {
            // 如果不连通，尝试修复，这里简单插入一个中间节点
            int newX = (x1 + x2) / 2;
            int newY = (y1 + y2) / 2;
            uint32_t newNode = newX + newY * m_width;
            if (m_map->getMapCell(newX, newY) != RMAP_OBSTACLE) {
                chrom.insert(chrom.begin() + i, newNode);
            }
        }
    }
}

// RPathPlan_astar类函数实现
RPathPlan_astar::RPathPlan_astar() {
    m_geneticData = nullptr;
    // 初始化m_scan，这里假设RRangeScan有默认构造函数
    m_scan = RRangeScan(); 
}

RPathPlan_astar::~RPathPlan_astar() {
    if (m_geneticData != nullptr) {
        delete m_geneticData;
        m_geneticData = nullptr;
    }
    // 这里可以添加释放m_scan相关资源的代码，如果有的话
}

int RPathPlan_astar::pathPlan() {
    // 实现路径规划逻辑
    // 初始化遗传算法相关数据
    if (m_map() != nullptr) {  // 假设RPathPlan类有获取地图指针的m_map()函数，需根据实际调整
        m_geneticData = new GeneticData(m_map());
        Population pop = m_geneticData->initPopulation(10);

        // 进行多轮遗传算法迭代
        const int numIterations = 50;
        for (int iter = 0; iter < numIterations; ++iter) {
            std::vector<double> fitnessValues;
            for (const auto& chrom : pop) {
                fitnessValues.push_back(m_geneticData->calculateFitness(chrom));
            }

            Population newPop;
            while (newPop.size() < pop.size()) {
                Chromosome parent1 = m_geneticData->selection(pop, fitnessValues);
                Chromosome parent2 = m_geneticData->selection(pop, fitnessValues);

                // 简单交叉操作，这里取两个染色体的前半部分和后半部分拼接
                size_t crossoverPoint = parent1.size() / 2;
                Chromosome child;
                for (size_t i = 0; i < crossoverPoint; ++i) {
                    child.push_back(parent1[i]);
                }
                for (size_t i = crossoverPoint; i < parent2.size(); ++i) {
                    child.push_back(parent2[i]);
                }

                // 简单变异操作，随机改变一个节点
                if (rand() % 100 < 10) {  // 10%的变异概率
                    size_t mutateIndex = rand() % child.size();
                    child[mutateIndex] = m_geneticData->generateValidNode();
                }

                newPop.push_back(child);
            }

            pop = newPop;
        }

        // 选择最优路径（这里简单取适应度最高的路径）
        double bestFitness = -1;
        Chromosome bestPath;
        for (const auto& chrom : pop) {
            double fitness = m_geneticData->calculateFitness(chrom);
            if (fitness > bestFitness) {
                bestFitness = fitness;
                bestPath = chrom;
            }
        }

        // 这里可以添加将最优路径进行处理或输出的代码
        // 例如将路径转换为实际的坐标点等操作

        return 0;
    }
    return -1;
}

int RPathPlan_astar::planBeg() {
    // 实现开始规划的逻辑
    // 例如初始化一些标志位或进行环境检查
    return 0; 
}

int RPathPlan_astar::planStep() {
    // 实现规划步骤的逻辑
    // 这里可以实现遗传算法的单步操作，如选择、交叉、变异等
    // 假设这里简单调用pathPlan中的部分逻辑进行单步操作
    if (m_geneticData != nullptr) {
        std::vector<double> fitnessValues;
        for (const auto& chrom : m_geneticData->initPopulation(10)) {
            fitnessValues.push_back(m_geneticData->calculateFitness(chrom));
        }
        Chromosome parent1 = m_geneticData->selection(m_geneticData->initPopulation(10), fitnessValues);
        Chromosome parent2 = m_geneticData->selection(m_geneticData->initPopulation(10), fitnessValues);

        // 简单交叉操作，这里取两个染色体的前半部分和后半部分拼接
        size_t crossoverPoint = parent1.size() / 2;
        Chromosome child;
        for (size_t i = 0; i < crossoverPoint; ++i) {
            child.push_back(parent1[i]);
        }
        for (size_t i = crossoverPoint; i < parent2.size(); ++i) {
            child.push_back(parent2[i]);
        }

        // 简单变异操作，随机改变一个节点
        if (rand() % 100 < 10) {  // 10%的变异概率
            size_t mutateIndex = rand() % child.size();
            child[mutateIndex] = m_geneticData->generateValidNode();
        }

        // 这里可以添加将新生成的路径进行处理或保存的代码
        return 0;
    }
    return -1;
}

int RPathPlan_astar::planStepBackward() {
    // 实现反向规划步骤的逻辑
    // 这里可以根据具体算法和需求来实现反向操作，目前简单返回0
    return 0; 
}

} // namespace rtk